import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy

from nav_msgs.msg import Odometry
from px4_msgs.msg import VehicleOdometry
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

class DroneOdom(Node):
    def __init__(self):
        super().__init__('odom_node')

        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.pub = self.create_publisher(Odometry,'odom_info',10)
        self.timer = self.create_timer(0.1, self.odom_timer_callback)

        self.sub = self.create_subscription(VehicleOdometry,'fmu/out/vehicle_odometry',self.odometry_callback,qos_profile)

        self.odom_broadcaster = TransformBroadcaster(self)

    def odom_timer_callback(self):
        try:
            self.pub.publish(self.odom)
        except:
            pass

    def odometry_callback(self, msgs):
        try:
            self.odom = Odometry()
            self.odom.header.stamp = self.get_clock().now().to_msg()
            self.odom.header.frame_id = 'base_link'
            self.odom.child_frame_id = 'drone_1'
            self.odom.pose.pose.position.x =  float(msgs.position[0])
            self.odom.pose.pose.position.y = -float(msgs.position[1])           
            self.odom.pose.pose.position.z = -float(msgs.position[2])

            # self.odom.pose.pose.orientation.w = float(msgs.q[0])
            # self.odom.pose.pose.orientation.x = float(msgs.q[1])           
            # self.odom.pose.pose.orientation.y = float(msgs.q[2])
            # self.odom.pose.pose.orientation.z = float(msgs.q[3])
            
            # 坐标变换：需要绕x轴旋转90度
            # (0.707+0.707i)(a+bi+cj+dk)(0.707-0.707i) = (a+bi-cj+dk)
            self.odom.pose.pose.orientation.w =  float(msgs.q[0])
            self.odom.pose.pose.orientation.x =  float(msgs.q[1])
            self.odom.pose.pose.orientation.y = -float(msgs.q[3])
            self.odom.pose.pose.orientation.z =  float(msgs.q[2])

            self.odom.twist.twist.linear.x =  float(msgs.velocity[0])
            self.odom.twist.twist.linear.y = -float(msgs.velocity[1])
            self.odom.twist.twist.linear.z = -float(msgs.velocity[2])

            self.odom.twist.twist.angular.x = float(msgs.angular_velocity[0])
            self.odom.twist.twist.angular.y = float(msgs.angular_velocity[1])
            self.odom.twist.twist.angular.z = float(msgs.angular_velocity[2])
        
        except:
            pass

def main(args = None):
    rclpy.init(args=args)
    node =  DroneOdom()
    rclpy.spin(node)
    rclpy.shutdown()